#include <iostream>
#include "spline.h"
#include "Bspline.h"
#include "matplotlibcpp.h"
#include <math.h>

namespace plt = matplotlibcpp;
using namespace std;

const double eps = 1e-16;

double X(double theta)
{
    return sqrt(3) * cos(theta);
}

double Y(double theta)
{
    return 2 * sqrt(3) * sin(theta) / 3 + 2 * sqrt(sqrt(3) * fabs(cos(theta))) / 3;
}

double curve_len(double l, double r)        //theta换算len
{    
    double mid = (l + r) / 2;
    double x1 = X(l), x2 = X(r), y1 = Y(l), y2 = Y(r), xm = X(mid), ym = Y(mid);
    double a = distance(x1,y1,x2,y2);
    double b = distance(x1,y1,xm,ym) + distance(x2,y2,xm,ym);
    return (fabs(b - a) < eps * 1000000) ? b : curve_len(l,mid) + curve_len(mid,r);
}

double curve_theta(double len)          //len换算theta
{
    double L = -0.5 * M_PI;
    double l = -0.5 * M_PI;
    double r = 1.5 * M_PI;
    double mid = (l + r) / 2;
    while(fabs(r - l) > eps * 1000000)
    {
        mid = (l + r) / 2;
        double mid_len = curve_len(L,mid);
        if (mid_len > len)
        {
            r = mid;
        }else if(mid_len < len)
        {
            l = mid;
        }else if(mid_len == len)
        {
            return mid;
        }
    }
    return mid;
}

int main()
{

    vector<int> N = {10,40,160};
    vector<curve_spline> C;

    double l = -0.5 * M_PI;
    double r = 1.5 * M_PI;
    double length = curve_len(l,r);


    for (int i = 0; i < N.size(); i++)
    {
        double gap = length / N[i];
        vector<double> x,y;
        for (int j = 0; j <= N[i]; j++)
        {
            double len = j * gap;       //以累积弦长为参数，取等距节点
            double theta = curve_theta(len);
            x.push_back(X(theta));
            y.push_back(Y(theta));
        }
        curve_spline C_i(x,y);
        C.push_back(C_i);
        C[i].fit();
    }

    //以下为画图部分
    double a = -1 * M_PI / 2, b = 3 * M_PI / 2; 
    int n = 1000;    //描点画图的点数
    double figgap = (b - a) / n;
    std::vector<double> u(n+1), x(n+1), y(n+1);

    plt::figure();          //创建一张图

    for (int j = 0; j <= n; j++)
    {
        u[j] = a + j * figgap;
        x[j] = X(u[j]);
        y[j] = Y(u[j]);
    }  
    plt::plot(x, y,{{"label", "origin"},{"color","k"},{"ls", "-"}});

    for (int k = 0; k < N.size(); k++)
    {
        double lengap = C[k].len() / n;
        for (int j = 0; j <= n; j++)
        {
            u[j] = 0 + j * lengap;
            x[j] = C[k].X(u[j]);
            y[j] = C[k].Y(u[j]);
        }  
        plt::plot(x, y,{{"label", "fit N="+to_string(N[k])},{"ls", "--"}});
    }

    plt::xlim(-2, 2);
    plt::title("spline of curve"); // set a title
    plt::legend();                // enable the legend

    plt::save("../fig/curve.png");
    cout << "output : ../fig/curve.png" <<endl;

    //单独作图
    for (int k = 0; k < N.size(); k++)
    {
        plt::figure();
        double lengap = C[k].len() / n;
        for (int j = 0; j <= n; j++)
        {
            u[j] = 0 + j * lengap;
            x[j] = C[k].X(u[j]);
            y[j] = C[k].Y(u[j]);
        }  
        plt::plot(x, y,{{"label", "fit N="+to_string(N[k])},{"ls", "-"}});
        plt::scatter<double,double>(C[k].x,C[k].y,(5.0),{{"label", "knots"},{"color","r"}});
        plt::xlim(-2, 2);
        plt::title("curve of N="+to_string(N[k])); // set a title
        plt::legend();                // enable the legend
        plt::save("../fig/curve"+to_string(k+1)+".png");
        cout << "output : ../fig/curve"+to_string(k+1)+".png" <<endl;
    }

    
    return 0;
}